#include "offboard.hpp"

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
}

void local_odom_callback(const nav_msgs::Odometry & current_msg)
{
    current_point_fcu.x= current_msg.pose.pose.position.x;
    current_point_fcu.y= current_msg.pose.pose.position.y;
    current_point_fcu.z= current_msg.pose.pose.position.z; //lidar offset z
    // current_point.z= current_point_z;


    current_angle_fcu.x= current_msg.pose.pose.orientation.x;
    current_angle_fcu.y= current_msg.pose.pose.orientation.y;
    current_angle_fcu.z= current_msg.pose.pose.orientation.z;
    current_angle_fcu.w= current_msg.pose.pose.orientation.w;
    curr_plane_angle_fcu = unionsys_core->toEulerAngle(current_angle_fcu);
    
    local_odom_flag = true;

}
void lio_odom_callback(const nav_msgs::Odometry & current_msg)
{
    current_point.x= current_msg.pose.pose.position.x;
    current_point.y= current_msg.pose.pose.position.y;
    current_point.z= current_msg.pose.pose.position.z+0.05; //lidar offset z
    // current_point.z= current_point_z;

    body_vel.x = current_msg.twist.twist.linear.x;
    body_vel.y = current_msg.twist.twist.linear.y;
    body_vel.z = current_msg.twist.twist.linear.z;

    current_angle.x= current_msg.pose.pose.orientation.x;
    current_angle.y= current_msg.pose.pose.orientation.y;
    current_angle.z= current_msg.pose.pose.orientation.z;
    current_angle.w= current_msg.pose.pose.orientation.w;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg.pose.pose.orientation.w, current_msg.pose.pose.orientation.x,
                                        current_msg.pose.pose.orientation.y, current_msg.pose.pose.orientation.z);

    local_odom_flag = true;
    
}



void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel1_value = rcvalue.channels[0]; //roll
    channel2_value = rcvalue.channels[1]; //pitch
    channel3_value = rcvalue.channels[2]; //thrust
    channel4_value = rcvalue.channels[3]; //yaw
    channel5_value = rcvalue.channels[4];
    channel6_value = rcvalue.channels[5];
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
}

void planWaypoint_callback(const quadrotor_msgs::PositionCommand &msg){
    planned_point.x= msg.position.x;
    planned_point.y= msg.position.y;
    planned_point.z= msg.position.z;
    planned_velocity.x = msg.velocity.x;
    planned_velocity.y = msg.velocity.y;
    planned_velocity.z = msg.velocity.z;
    planned_yaw_angle = msg.yaw;
    planned_yaw_dot = msg.yaw_dot;
    planned_update_flag = true;
}

void velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vel.x = msg.twist.linear.x;
    current_vel.y = msg.twist.linear.y;
    current_vel.z = msg.twist.linear.z;
    if(sqrt(current_vel.x*current_vel.x+current_vel.y*current_vel.y+current_vel.z*current_vel.z)<0.2)
    {
        vehicle_static_time = ros::Time::now().toSec();
    }
    else
    {
        vehicle_go_time = ros::Time::now().toSec();
    }
    
}

void human_pose_callback(const geometry_msgs::Point &msg)
{
    human_target_distance = sqrt((msg.y-current_point.y)*(msg.y-current_point.y)+(msg.x-current_point.x)*(msg.x-current_point.x)+(msg.z-current_point.z)*(msg.z-current_point.z));
    target_point_human.x = msg.x;
    target_point_human.y = msg.y;
    target_point_human.z = msg.z;
    human_update_time = ros::Time::now().toSec();

}

void car_pose_callback(const geometry_msgs::Point &msg)
{
    car_target_distance = sqrt((msg.y-current_point.y)*(msg.y-current_point.y)+(msg.x-current_point.x)*(msg.x-current_point.x)+(msg.z-current_point.z)*(msg.z-current_point.z));
    target_point_car.x = msg.x;
    target_point_car.y = msg.y;
    target_point_car.z = msg.z;
    car_update_time = ros::Time::now().toSec();
}

void oil_pose_callback(const geometry_msgs::Point &msg)
{
    oil_target_distance = sqrt((msg.y-current_point.y)*(msg.y-current_point.y)+(msg.x-current_point.x)*(msg.x-current_point.x)+(msg.z-current_point.z)*(msg.z-current_point.z));
    target_point_oil.x = msg.x;
    target_point_oil.y = msg.y;
    target_point_oil.z = msg.z;
    oil_update_time = ros::Time::now().toSec();
    if(mission_number<=7)
    {
        geometry_msgs::Point oil_target_position;
        oil_target_position.x = 8.5-self_offset.x;
        oil_target_position.y = -30.5-self_offset.y;
        oil_target_position.z = 0.7-self_offset.z;
        geometry_msgs::Point lio_offset_byoil;
        lio_offset_byoil.x = target_point_oil.x-oil_target_position.x;
        lio_offset_byoil.y = target_point_oil.y-oil_target_position.y;
        lio_offset_byoil.z = target_point_oil.z-oil_target_position.z;
        double distance_offset = sqrt((lio_offset_byoil.x*lio_offset_byoil.x)+(lio_offset_byoil.y*lio_offset_byoil.y)+(lio_offset_byoil.z*lio_offset_byoil.z));
        if(distance_offset<5)
        {
            if(oil_target_distance<nearest_oil_offset)
            {
                nearest_oil_offset = oil_target_distance;
                nearest_lio_offset_byoil = lio_offset_byoil;
                std::cout<<"=====>offset: "<<nearest_lio_offset_byoil.x<<","<<nearest_lio_offset_byoil.y<<","<<nearest_lio_offset_byoil.z<<std::endl;
                pub_planner_waypoints_once = true;

            }
            
        }
    }
    
    
}
void weapon_pose_callback(const geometry_msgs::Point &msg)
{
    weapon_target_distance = sqrt((msg.y-current_point.y)*(msg.y-current_point.y)+(msg.x-current_point.x)*(msg.x-current_point.x)+(msg.z-current_point.z)*(msg.z-current_point.z));
    target_point_weapon.x = msg.x;
    target_point_weapon.y = msg.y;
    target_point_weapon.z = msg.z;
    weapon_update_time = ros::Time::now().toSec();
}


void exploration_finished_callback(const std_msgs::Bool &msg)
{
    exploration_finished_flag = true;
}
void start_mission_callback(const geometry_msgs::PoseStamped &msg)
{
    std::string str = msg.header.frame_id;
    int drone_id_msg = atoi(str.c_str());
    if(drone_id_msg==300+(ceil(mission_number/6)))
    {
        std::cout<<"--->get mission flag"<<std::endl;
        get_mission_flag = true;
    }
    if(drone_id_msg==303)
    {
        if(mission_number>=13)
        {
           std::cout<<"--->get mission flag"<<std::endl;
            get_mission_flag = true; 
        }
    }
    if( drone_id_msg==100||drone_id_msg==(mission_number+100))
    {
       if(get_mission_flag)
       {
        //start mission 
        std::cout<<"--->start mission flag"<<std::endl;
        start_mission_flag = true;
       }
        
    }
    else if(drone_id_msg==mission_number)
    {
        target_goal_from_gc.x = msg.pose.position.x;
        target_goal_from_gc.y = msg.pose.position.y;
        target_goal_from_gc.z = msg.pose.position.z;
        target_goal_gc_flag = true;
    }
    
}

void stop_to_bizhang_callback(const std_msgs::Bool &msg)
{
    stop_to_bizhang_flag = msg.data;
}

void outdoor_return_callback(const std_msgs::UInt8 &msg)
{
    if(msg.data==mission_number||msg.data==100)
    {
        return_flag = true;
    }
}

void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    if(print_state_cnt>20)
    {
        //print state per second
        printFSMExecState();
        print_state_cnt = 0;
        std_msgs::Float32MultiArray fsm_state;
        fsm_state.data.push_back(mission_number);
        fsm_state.data.push_back(exec_state);
        fsm_state.data.push_back(channel7_value);
        fsm_state_pub.publish(fsm_state);
    }
    print_state_cnt++;
    
    switch (exec_state) { 
    case INIT: {
        if (!local_odom_flag) {
            break;
        }
        changeFSMExecState(CRUISE, "offboard_fsm");
    break;
    }
    case CRUISE: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);

        // std_msgs::UInt8 chuzhi_number;
        // chuzhi_number.data = 1;
        // chuzhi_msg_pub.publish(chuzhi_number);
        
        if(mission_number<=12)
        {

            set_cruise_state();
            
            if(current_state.mode == "OFFBOARD")
            {
                if(start_mission_flag||channel7_value>1800)
                {
                    if(!stop_to_bizhang_flag)
                    {
                        changeFSMExecState(TAKEOFF, "offboard_fsm");
                    }
                    else
                    {
                        std::cout<<"------>wait for id-1 drone leaving..."<<std::endl;
                    }
                    
                }
                
            }
        }
        else
        {
            if(channel8_value>1800)
            {
                if(start_mission_flag||channel7_value>1800)
                {
                    changeFSMExecState(MISSION, "offboard_fsm");
                }
            }
        }
        
        
    break;
    }
    case MISSION: {
        if(channel8_value>1800&&current_state.mode != "AUTO.RTL"&&!current_state.armed && (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
            if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                ROS_INFO("Vehicle armed...");
            }
            last_offboard_request = ros::Time::now();
        }
        // 切换至任务模式
        mavros_msgs::SetMode mission_set_mode;
        mission_set_mode.request.custom_mode = "AUTO.MISSION";
        if(channel8_value>1800&&current_state.armed&&current_state.mode != "AUTO.RTL"&&current_state.mode != "AUTO.MISSION"&& (ros::Time::now() - last_offboard_request > ros::Duration(1.0)))
        {
            if (set_mode_client.call(mission_set_mode) && mission_set_mode.response.mode_sent) 
            {
                ROS_INFO("Mission mode enabled");
            } 
            last_offboard_request = ros::Time::now();
        }

        if(return_flag)
        {
            changeFSMExecState(RETURN, "offboard_fsm");
        }
        


    break;
    }

    case RETURN:{
        // 切换到返航模式
        mavros_msgs::SetMode rtl_set_mode;
        rtl_set_mode.request.custom_mode = "AUTO.RTL";
        
        // 条件检查：无人机已解锁、当前模式不是返航模式、且距离上次请求超过1秒
        if(current_state.armed && 
           current_state.mode != "AUTO.RTL" && 
           (ros::Time::now() - last_offboard_request > ros::Duration(1.0)))
        {
            if(set_mode_client.call(rtl_set_mode) && rtl_set_mode.response.mode_sent){
                ROS_INFO("Return mode (RTL) enabled");
            }
            last_offboard_request = ros::Time::now();
        }

        // 可选：检测是否完成返航，切换至其他状态（如降落或待机）
        // if(返航完成条件){
        //     changeFSMExecState(LAND, "offboard_fsm");
        // }
        break;

    }

    case TAKEOFF: {
        if( !current_state.armed && (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed...");
            }
            last_offboard_request = ros::Time::now();
        }
        cruise_point.x = 0;
        cruise_point.y = 0;
        cruise_point.z = 2.0;
        position_pid_control(cruise_point,current_point,0.5,0,curr_plane_angle.z,false);
        vel_command_send(velocity_expected, 0);

        if(current_point.z>1.5)
        {
            waypoint_exec_num = 0;
            vehicle_go_time = ros::Time::now().toSec();
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
        }
    break;
    }



    case PLANNER_MULTI: {

        if(mission_number<=12)
        {
            max_vel_beishu = 0.5;
        }
        else
        {
            max_vel_beishu = 2.0;
        }
        
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        std_msgs::Float32MultiArray virtual_ceil_ground_msg;
        // if(waypoint_exec_num==0||waypoint_exec_num==1||waypoint_exec_num==2||waypoint_exec_num==3||waypoint_exec_num==waypoints_planner_multi.waypoints.size()-3||waypoint_exec_num==waypoints_planner_multi.waypoints.size()-2||waypoint_exec_num==waypoints_planner_multi.waypoints.size()-1)
        // {
        //     virtual_ceil_ground_msg.data.push_back(6.0);
        //     virtual_ceil_ground_msg.data.push_back(0.1);
        // }
        // else
        // {
        //     virtual_ceil_ground_msg.data.push_back(waypoints_planner_multi.waypoints[waypoint_exec_num].z_alt+nearest_lio_offset_byoil.z+1.0);
        //     virtual_ceil_ground_msg.data.push_back(waypoints_planner_multi.waypoints[waypoint_exec_num].z_alt+nearest_lio_offset_byoil.z-1.5);
        // }
        virtual_ceil_ground_msg.data.push_back(14.0);
        virtual_ceil_ground_msg.data.push_back(0.1);
        
        virtual_ceil_ground_pub.publish(virtual_ceil_ground_msg);
        waypoints_planner(waypoints_planner_multi, 0.0, true);
        // if(mission_number<=12)
        // {
        //     waypoints_planner(waypoints_planner_multi, 0.0, true);
        // }
        // else
        // {
        //     //室外侦察飞机飞行控制
        //     waypoints_no_planner(waypoints_planner_multi, 0.0, 0.5);
        // }
        

        if(stop_to_bizhang_flag)
        {
            changeFSMExecState(STOP, "offboard_fsm");
        }
        if(target_goal_gc_flag)
        {
            
            changeFSMExecState(PLANNER_SINGLE, "offboard_fsm");
        }

        // if(ros::Time::now().toSec()-human_update_time<1.0&&human_target_distance<5.0)
        // {
        //     int near_cnt=0;
        //     for(int kk=0;kk<human_chuzhi_list.size();kk++)
        //     {
        //         double dist_to_list_target = sqrt((target_point_human.x-human_chuzhi_list[kk].x)*(target_point_human.x-human_chuzhi_list[kk].x)+
        //                                             (target_point_human.y-human_chuzhi_list[kk].y)*(target_point_human.y-human_chuzhi_list[kk].y));
        //         if(dist_to_list_target<3.0)
        //         {
        //             near_cnt++;
        //         }
        //     }
        //     if(near_cnt==0)
        //     {
        //         changeFSMExecState(CHUZHI_HUMAN, "offboard_fsm");
        //     }
            
        // }
        // if(ros::Time::now().toSec()-car_update_time<1.0&&car_target_distance<5.0&&mission_number==7)
        // {
        //     if(!car_chuzhi_finish_flag)
        //     {
        //         changeFSMExecState(CHUZHI_CAR, "offboard_fsm");
        //     }
        // }
        // if(ros::Time::now().toSec()-oil_update_time<1.0&&oil_target_distance<5.0)
        // {
        //     int near_cnt=0;
        //     for(int kk=0;kk<oil_chuzhi_list.size();kk++)
        //     {
        //         double dist_to_list_target = sqrt((target_point_oil.x-oil_chuzhi_list[kk].x)*(target_point_oil.x-oil_chuzhi_list[kk].x)+
        //                                             (target_point_oil.y-oil_chuzhi_list[kk].y)*(target_point_oil.y-oil_chuzhi_list[kk].y));
        //         if(dist_to_list_target<3.0)
        //         {
        //             near_cnt++;
        //         }
        //     }
        //     if(near_cnt==0)
        //     {
        //         changeFSMExecState(CHUZHI_OIL, "offboard_fsm");
        //     }
        // }
        // if(ros::Time::now().toSec()-weapon_update_time<1.0&&weapon_target_distance<5.0)
        // {
        //     int near_cnt=0;
        //     for(int kk=0;kk<weapon_chuzhi_list.size();kk++)
        //     {
        //         double dist_to_list_target = sqrt((target_point_weapon.x-weapon_chuzhi_list[kk].x)*(target_point_weapon.x-weapon_chuzhi_list[kk].x)+
        //                                             (target_point_weapon.y-weapon_chuzhi_list[kk].y)*(target_point_weapon.y-weapon_chuzhi_list[kk].y));
        //         if(dist_to_list_target<3.0)
        //         {
        //             near_cnt++;
        //         }
        //     }
        //     if(near_cnt==0)
        //     {
        //         changeFSMExecState(CHUZHI_WEAPON, "offboard_fsm");
        //     }
        // }

    break;
    }
    case CHUZHI_HUMAN: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);

        double target_dist = 2.0;
        double theata_vehicle_to_target = atan2(target_point_human.y-current_point.y, target_point_human.x-current_point.x);
        geometry_msgs::Point tracing_human_point;
        tracing_human_point.x = target_point_human.x-target_dist*cos(theata_vehicle_to_target);
        tracing_human_point.y = target_point_human.y-target_dist*sin(theata_vehicle_to_target);
        tracing_human_point.z = waypoints_planner_multi.waypoints[waypoint_exec_num].z_alt;
        
        double distance_to_human = sqrt((tracing_human_point.x-current_point.x)*(tracing_human_point.x-current_point.x)
                                                +(tracing_human_point.y-current_point.y)*(tracing_human_point.y-current_point.y));
        
        
        if(distance_to_human<=(0.5)&&!human_chuzhi_start_flag)
        {
            human_chuzhi_time = ros::Time::now().toSec();
            human_chuzhi_start_flag = true;
            
        }
        if(!human_chuzhi_start_flag)
        {
            simple_goal_planner(tracing_human_point.x, tracing_human_point.y,tracing_human_point.z, 0, true);
        }
        else
        {
            position_pid_control(tracing_human_point,current_point,max_vel_beishu,0.0,curr_plane_angle.z,false);
            vel_command_send(velocity_expected, attitude_expect.z);
        }
        if(human_chuzhi_start_flag&&ros::Time::now().toSec()-human_chuzhi_time>10.0)
        {
            human_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 1;
            chuzhi_msg_pub.publish(chuzhi_number);

            human_chuzhi_list.push_back(target_point_human);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            vehicle_go_time = ros::Time::now().toSec();
            human_chuzhi_start_flag = false;
        }
        if(ros::Time::now().toSec()-human_update_time>5.0)
        {
            human_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 1;
            chuzhi_msg_pub.publish(chuzhi_number);
            human_chuzhi_list.push_back(target_point_human);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            vehicle_go_time = ros::Time::now().toSec();
            human_chuzhi_start_flag = false;
        }
    break;
    }
    
    case CHUZHI_CAR: {
        max_vel_beishu = 1.0;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        geometry_msgs::Point tracing_point;
        tracing_point.x = target_point_car.x;
        tracing_point.y = target_point_car.y;
        tracing_point.z = 1.5;

        position_pid_control(tracing_point,current_point,max_vel_beishu,0.0,curr_plane_angle.z,false);
        vel_command_send(velocity_expected, attitude_expect.z);
        double distance_to_car = sqrt((target_point_car.x-current_point.x)*(target_point_car.x-current_point.x)
                                                +(target_point_car.y-current_point.y)*(target_point_car.y-current_point.y));
        if(distance_to_car<0.5&&!car_chuzhi_start_flag)
        {
            car_chuzhi_time = ros::Time::now().toSec();
            car_chuzhi_start_flag = true;
            
        }
        if(car_chuzhi_start_flag)
        {
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 2;
            chuzhi_msg_pub.publish(chuzhi_number);
        }
        if(car_chuzhi_start_flag&&ros::Time::now().toSec()-car_chuzhi_time>20.0)
        {
            car_chuzhi_finish_flag=true;
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            vehicle_go_time = ros::Time::now().toSec();
        }
        if(ros::Time::now().toSec()-car_update_time>5.0)
        {
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            vehicle_go_time = ros::Time::now().toSec();
        }
    break;
    }
    case CHUZHI_OIL: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        double target_dist = 1.5;
        double theata_vehicle_to_target = atan2(target_point_oil.y-current_point.y, target_point_oil.x-current_point.x);
        geometry_msgs::Point tracing_oil_point;
        tracing_oil_point.x = target_point_oil.x-target_dist*cos(theata_vehicle_to_target);
        tracing_oil_point.y = target_point_oil.y-target_dist*sin(theata_vehicle_to_target);
        tracing_oil_point.z = waypoints_planner_multi.waypoints[waypoint_exec_num].z_alt;
        
        double distance_to_target = sqrt((tracing_oil_point.x-current_point.x)*(tracing_oil_point.x-current_point.x)
                                                +(tracing_oil_point.y-current_point.y)*(tracing_oil_point.y-current_point.y));
        
        
        if(distance_to_target<=(0.5)&&!oil_chuzhi_start_flag)
        {
            oil_chuzhi_time = ros::Time::now().toSec();
            oil_chuzhi_start_flag = true;
            
        }
        if(!oil_chuzhi_start_flag)
        {
            simple_goal_planner(tracing_oil_point.x, tracing_oil_point.y,tracing_oil_point.z, 0, true);
        }
        else
        {
            position_pid_control(tracing_oil_point,current_point,max_vel_beishu,0.0,curr_plane_angle.z,false);
            vel_command_send(velocity_expected, attitude_expect.z);
        }
        if(oil_chuzhi_start_flag&&ros::Time::now().toSec()-oil_chuzhi_time>10.0)
        {
            oil_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 3;
            chuzhi_msg_pub.publish(chuzhi_number);
            oil_chuzhi_list.push_back(target_point_oil);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            oil_chuzhi_start_flag = false;
            vehicle_go_time = ros::Time::now().toSec();
        }
        if(ros::Time::now().toSec()-oil_update_time>5.0)
        {
            oil_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 3;
            chuzhi_msg_pub.publish(chuzhi_number);
            oil_chuzhi_list.push_back(target_point_oil);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            oil_chuzhi_start_flag = false;
            vehicle_go_time = ros::Time::now().toSec();
        }
    break;
    }
    case CHUZHI_WEAPON: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        double target_dist = 1.5;
        double theata_vehicle_to_target = atan2(target_point_weapon.y-current_point.y, target_point_weapon.x-current_point.x);
        geometry_msgs::Point tracing_weapon_point;
        tracing_weapon_point.x = target_point_weapon.x-target_dist*cos(theata_vehicle_to_target);
        tracing_weapon_point.y = target_point_weapon.y-target_dist*sin(theata_vehicle_to_target);
        tracing_weapon_point.z = waypoints_planner_multi.waypoints[waypoint_exec_num].z_alt;
        
        double distance_to_target = sqrt((tracing_weapon_point.x-current_point.x)*(tracing_weapon_point.x-current_point.x)
                                                +(tracing_weapon_point.y-current_point.y)*(tracing_weapon_point.y-current_point.y));
        
        
        if(distance_to_target<=(0.5)&&!weapon_chuzhi_start_flag)
        {
            weapon_chuzhi_time = ros::Time::now().toSec();
            weapon_chuzhi_start_flag = true;
        }
        if(!weapon_chuzhi_start_flag)
        {
            simple_goal_planner(tracing_weapon_point.x, tracing_weapon_point.y,tracing_weapon_point.z, 0, true);
        }
        else
        {
            position_pid_control(tracing_weapon_point,current_point,max_vel_beishu,0.0,curr_plane_angle.z,false);
            vel_command_send(velocity_expected, attitude_expect.z);
        }
        if(weapon_chuzhi_start_flag&&ros::Time::now().toSec()-weapon_chuzhi_time>10.0)
        {
            weapon_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 4;
            chuzhi_msg_pub.publish(chuzhi_number);
            weapon_chuzhi_list.push_back(target_point_weapon);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            weapon_chuzhi_start_flag = false;
            vehicle_go_time = ros::Time::now().toSec();
        }
        if(ros::Time::now().toSec()-weapon_update_time>5.0)
        {
            weapon_chuzhi_finish_flag++;
            std_msgs::UInt8 chuzhi_number;
            chuzhi_number.data = 4;
            chuzhi_msg_pub.publish(chuzhi_number);
            weapon_chuzhi_list.push_back(target_point_weapon);
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
            weapon_chuzhi_start_flag = false;
            vehicle_go_time = ros::Time::now().toSec();
        }
    break;
    }

    case PLANNER_SINGLE: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        planner_target_point_x = target_goal_from_gc.x;
        planner_target_point_y = target_goal_from_gc.y;
        planner_target_point_z = 1.5;
        simple_goal_planner(planner_target_point_x, planner_target_point_y, planner_target_point_z, 0, true);
        double distance_to_planner = sqrt((planner_target_point_x-current_point.x)*(planner_target_point_x-current_point.x)
                                                +(planner_target_point_y-current_point.y)*(planner_target_point_y-current_point.y));
        // if(distance_to_planner<0.5&&abs(current_vel.x)<0.2&&abs(current_vel.y)<0.2&&abs(current_vel.z)<0.2)
        // {
        //     changeFSMExecState(GO_EXPLORATION, "offboard_fsm");
        // }
    break;
    }

    
    case STOP:{
        set_cruise_state();
        if(!stop_to_bizhang_flag)
        {
            pub_planner_waypoints_once = true;
            vehicle_go_time =  ros::Time::now().toSec();
            changeFSMExecState(PLANNER_MULTI, "offboard_fsm");
        }
        break;
    }

    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file
    nh.param<double>("mission_number",mission_number,false);
    nh.param<bool>("use_sitl",use_sitl,false);
    nh.param<double>("max_vel_beishu",max_vel_beishu,1);


    nh.param<double>("planner_target_point_x",planner_target_point_x,1);
    nh.param<double>("planner_target_point_y",planner_target_point_y,1);
    nh.param<double>("planner_target_point_z",planner_target_point_z,1);

    nh.param<double>("exploration_start_point_x",exploration_start_point_x,1);
    nh.param<double>("exploration_start_point_y",exploration_start_point_y,1);
    nh.param<double>("exploration_start_point_z",exploration_start_point_z,1);
    nh.param<double>("exploration_start_yaw",exploration_start_yaw,1);

    nh.param<double>("exploration_min_x",exploration_min_x,1);
    nh.param<double>("exploration_min_y",exploration_min_y,1);
    nh.param<double>("exploration_max_x",exploration_max_x,1);
    nh.param<double>("exploration_max_y",exploration_max_y,1);
    

    nh.param<double>("endpoint_point_x",endpoint_point_x,1);
    nh.param<double>("endpoint_point_y",endpoint_point_y,1);
    nh.param<double>("endpoint_point_z",endpoint_point_z,1);

    //Subscriber
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    planWaypointsub = nh.subscribe("/planning/pos_cmd", 10, planWaypoint_callback);
    velocitysub     = nh.subscribe("/mavros/local_position/velocity_local",10,velocity_callback);
    lio_odom_sub = nh.subscribe("/Odometry",10,lio_odom_callback);
    local_odom_sub = nh.subscribe("/mavros/local_position/odom",10,local_odom_callback);
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);
    state_sub = nh.subscribe("mavros/state", 10, state_cb);

    //subscribe from yolo
    human_pose_sub = nh.subscribe("/zhou/pose_human",10,human_pose_callback);
    car_pose_sub = nh.subscribe("/zhou/pose_car",10,car_pose_callback);
    oil_pose_sub = nh.subscribe("/zhou/pose_oil",10,oil_pose_callback);
    weapon_pose_sub = nh.subscribe("/zhou/pose_weapon",10,weapon_pose_callback);

    exploration_sub = nh.subscribe("/zhou/exploration_finished",10,exploration_finished_callback);

    startmission_sub = nh.subscribe("/zhou/start_mission",10,start_mission_callback);
    stoptobizhang_sub = nh.subscribe("/zhou/need_stoptoavoid",10,stop_to_bizhang_callback);

    return_flag_sub = nh.subscribe("/zhou/outdoor_return",10,outdoor_return_callback);

    //Publisher
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
    simple_goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    waypoints_pub = nh.advertise<mavros_msgs::WaypointList>("/zhou/waypoints", 10);
    // new_plan_pub = nh.advertise<std_msgs::Bool>("/zhou/neednewplan",10);
    max_vel_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/max_vel",10);
    virtual_ceil_ground_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/virtual_ceil_ground",10);

    exploration_msg_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/exploration",10);

    chuzhi_msg_pub = nh.advertise<std_msgs::UInt8>("/zhou/chuzhi_msg",10);
    fsm_state_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/vechicle_state",10);
    


    control_plot = nh.advertise<std_msgs::Float32MultiArray>("/plot/controller",10);


    //Service
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("mavros/setpoint_velocity/mav_frame");

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.05), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(30);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"------------>wait for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;
    setmavframe.request.mav_frame = 8;
    last_offboard_request = ros::Time::now();
    //set mavframe to body_ned 8
    if(set_vechile_ned.call(setmavframe) && setmavframe.response.success)
    {
        std::cout<<"------------>setmavframe success..."<<std::endl;
    }
    else{
        std::cout<<"------------>setmavframe failed --------"<<std::endl;
    }
    self_offset = vehicle_offset[mission_number-1];
    //decide which waypoints to go
    if(mission_number == 1)
    {
        waypoints_self = waypoints_1;
    }
    else if(mission_number == 2)
    {
        waypoints_self = waypoints_2;
    }
    else if(mission_number == 3)
    {
        waypoints_self = waypoints_3;
    }
    else if(mission_number == 4)
    {
        waypoints_self = waypoints_4;
    }
    else if(mission_number == 5)
    {
        waypoints_self = waypoints_5;
    }
    else if(mission_number == 6)
    {
        waypoints_self = waypoints_6;
    }
    else if(mission_number == 7)
    {
        waypoints_self = waypoints_7;
    }
    else if(mission_number == 8)
    {
        waypoints_self = waypoints_8;
    }
    else if(mission_number == 9)
    {
        waypoints_self = waypoints_9;
    }
    else if(mission_number == 10)
    {
        waypoints_self = waypoints_10;
    }
    else if(mission_number == 11)
    {
        waypoints_self = waypoints_11;
    }
    else if(mission_number == 12)
    {
        waypoints_self = waypoints_12;
    }
    else if(mission_number == 13)
    {
        waypoints_self = waypoints_13;
    }
    else if(mission_number == 14)
    {
        waypoints_self = waypoints_14;
    }
    else if(mission_number == 15)
    {
        waypoints_self = waypoints_15;
    }
    else if(mission_number == 16)
    {
        waypoints_self = waypoints_16;
    }
    else if(mission_number == 17)
    {
        waypoints_self = waypoints_17;
    }
    else if(mission_number == 18)
    {
        waypoints_self = waypoints_18;
    }
    else if(mission_number == 19)
    {
        waypoints_self = waypoints_19;
    }
    else if(mission_number == 20)
    {
        waypoints_self = waypoints_20; 
    }
    else if(mission_number == 21)
    {
        waypoints_self = waypoints_21;
    }
    else if(mission_number == 22)
    {
        waypoints_self = waypoints_22;
    }


    for(int i=0;i<waypoints_self.size();i++)
    {
        mavros_msgs::Waypoint waypoint_temp;
        waypoint_temp.x_lat = waypoints_self[i].x-self_offset.x;
        waypoint_temp.y_long = waypoints_self[i].y-self_offset.y;
        waypoint_temp.z_alt = waypoints_self[i].z-self_offset.z;
        waypoints_planner_multi.waypoints.push_back(waypoint_temp);
    }
    // mavros_msgs::Waypoint waypoint_home;
    // waypoint_home.x_lat = 0.0;
    // waypoint_home.y_long = 0.0;
    // waypoint_home.z_alt = 1.0;
    // waypoints_planner_multi.waypoints.push_back(waypoint_home);
    nearest_lio_offset_byoil.x = 0.0;
    nearest_lio_offset_byoil.y = 0.0;
    nearest_lio_offset_byoil.z = 0.0;
    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

void vel_command_send(geometry_msgs::Point vel, double yaw_rate)
{
    vel_command_cnt++;
    geometry_msgs::Vector3 linear_temp;
    geometry_msgs::Vector3 angular_temp;

    geometry_msgs::TwistStamped msg_vel_pub;
    linear_temp.x = vel.x;
    linear_temp.y = vel.y;
    linear_temp.z = vel.z;
    
    angular_temp.x = 0;
    angular_temp.y = 0;
    angular_temp.z = yaw_rate;
    msg_vel_pub.header.stamp = ros::Time::now();
    msg_vel_pub.header.seq=vel_command_cnt;
    msg_vel_pub.twist.linear=linear_temp;
    msg_vel_pub.twist.angular=angular_temp;
    local_vel_pub.publish(msg_vel_pub);
}


void changeFSMExecState(FSM_STATE new_state, std::string pos_call) {

  int    pre_s        = int(exec_state);
  exec_state         = new_state;
  std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
  std::cout << "state:" + state_str[int(exec_state)] << std::endl;
}


void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float self_yaw, bool ifplan)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0, pitch = 0.0;
    geometry_msgs::Point error_pos;
    float dt = ros::Time::now().toSec()-last_calc_time;
    if(dt>1.0) //dt too long maybe need init dt
    {
        last_calc_time = ros::Time::now().toSec();
        return;
    }
    //position control  mode & 0x01
    if (1) {
        //calculate velocity, P control
        
        error_pos.x = current_set_point.x - current_local_point.x;
        error_pos.y = current_set_point.y - current_local_point.y;
        if(!ifplan)
        {
            if(abs(error_pos.x)<0.3)
            {
                error_pos.x = 0.0;
            }
            if(abs(error_pos.y)<0.3)
            {
                error_pos.y = 0.0;
            }
        }
        vxp = P_pos * error_pos.x;
        vxd = -(D_pos * current_vel.x);
        vyp = P_pos * error_pos.y;
        vyd = -(D_pos * current_vel.y);
        
        

        if (abs(error_pos.x) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.x += error_pos.x*dt;
        } else {
            error_pos_integrated.x = 0.0;
        }
        if (abs(error_pos.y) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.y += error_pos.y*dt;
        } else {
            error_pos_integrated.y = 0.0;
        }
        
        error_pos_integrated.x = limit((double) error_pos_integrated.x, max_vel_beishu*1.0 / I_pos);
        error_pos_integrated.y = limit((double) error_pos_integrated.y, max_vel_beishu*1.0 / I_pos);
        
        vxi = I_pos * error_pos_integrated.x;
        vyi = I_pos * error_pos_integrated.y;
        
        if(ifplan)
        {
            vx = (vxp + vxi + vxd)+planned_velocity.x;
            vy = (vyp + vyi + vyd)+planned_velocity.y;
        }
        else
        {
            vx = vxp;
            vy = vyp;
        }
        
        //limit the speed
        // vx_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).x;
        // vy_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).y;
        vx_lim = limit(vx, velocity_limit);
        vy_lim = limit(vy, velocity_limit);
        //ROS_INFO("vx_exp vy_exp %f %f",vx,vy);

        error_pos_last = error_pos;
    }
    //yaw control  mode & 0x02
    if (1) {
        //use the flight controller yaw in default
        
        //Drone turns at the smallest angle
        float error_yaw = (target_yaw - self_yaw) * rad2deg;
        if (error_yaw < -180)
            error_yaw += 360;
        else if (error_yaw > 180)
            error_yaw -= 360;
        yaw_rate_p = P_yaw * error_yaw;
        yaw_rate_d = D_yaw * (error_yaw - error_yaw_last);
        error_yaw_integrated += error_yaw;
        if (I_yaw > 0.0001) {
            error_yaw_integrated = limit(error_yaw_integrated, YAW_I_LIMIT / I_yaw);
        }
        yaw_rate_i = I_yaw * error_yaw_integrated;
        if (abs(error_yaw) <= YAW_DEAD_ZONE) {
            yaw_rate_p = 0.0;
            yaw_rate_i = 0.0;
            yaw_rate_d = 0.0;
            error_yaw_integrated = 0.0;
        }
        yaw_rate = (yaw_rate_p + yaw_rate_i + yaw_rate_d);
        if (abs(error_yaw) >= YAW_RATE_LIMIT) {
            yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
        } 
        error_yaw_last = error_yaw;
    }

    //height control  mode & 0x04
    if (1) {
        float deltaz = current_local_point.z - current_set_point.z;
        if (fabs(deltaz) >= HEIGHT_DEAD_ZONE) {
            vz = P_z * (current_set_point.z - current_local_point.z);
        } else {
            vz = 0.0;
        }
    }
    velocity_expected.x = vx_lim * cos(self_yaw) + vy_lim * sin(self_yaw);
    velocity_expected.y = vy_lim * cos(self_yaw) - vx_lim * sin(self_yaw);
    velocity_expected.z = vz;
    attitude_expect.z = yaw_rate * deg2rad;
    last_calc_time = ros::Time::now().toSec();


}

void set_cruise_state()
{
    if(sqrt(current_vel.x*current_vel.x+current_vel.y*current_vel.y+current_vel.z*current_vel.z)<0.2)
    {
        position_pid_control(cruise_point,current_point,0.5,0,curr_plane_angle.z,false);
        vel_command_send(velocity_expected, attitude_expect.z);
    }
    else
    {
        cruise_point.x = current_point.x;
        cruise_point.y = current_point.y;
        cruise_point.z = current_point.z;
        velocity_expected.x = 0.0;
        velocity_expected.y = 0.0;
        velocity_expected.z = 0.0;
        vel_command_send(velocity_expected, 0);
    }
}

void waypoints_no_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, double yuzhi)
{
    geometry_msgs::Point this_point;
    this_point.x = waypoints_plan.waypoints[waypoint_exec_num].x_lat;
    this_point.y = waypoints_plan.waypoints[waypoint_exec_num].y_long;
    this_point.z = waypoints_plan.waypoints[waypoint_exec_num].z_alt;

    position_pid_control(this_point,current_point_fcu,max_vel_beishu,yaw,curr_plane_angle_fcu.z,false);
    vel_command_send(velocity_expected, attitude_expect.z);

    double distance_to_go_nextpoint = sqrt((waypoints_plan.waypoints[waypoint_exec_num].x_lat-current_point_fcu.x)*(waypoints_plan.waypoints[waypoint_exec_num].x_lat-current_point_fcu.x)
                                                +(waypoints_plan.waypoints[waypoint_exec_num].y_long-current_point_fcu.y)*(waypoints_plan.waypoints[waypoint_exec_num].y_long-current_point_fcu.y));
    if(distance_to_go_nextpoint<yuzhi)
    {
        if(waypoint_exec_num<waypoints_plan.waypoints.size()-1)
        {
            waypoint_exec_num++;
        }
        
    }


}

void waypoints_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, bool specified_yaw)
{
    geometry_msgs::PoseStamped goal_msg;
    goal_msg.pose.position.x = waypoints_plan.waypoints[waypoint_exec_num].x_lat+nearest_lio_offset_byoil.x;
    goal_msg.pose.position.y = waypoints_plan.waypoints[waypoint_exec_num].y_long+nearest_lio_offset_byoil.y;
    goal_msg.pose.position.z = waypoints_plan.waypoints[waypoint_exec_num].z_alt+nearest_lio_offset_byoil.z;
    if(pub_planner_waypoints_once)
    {
        std::cout<<"---->goal position: "<<goal_msg.pose.position.x<<","<<goal_msg.pose.position.y<<std::endl;
        simple_goal_pub_.publish(goal_msg);
        last_path_send = ros::Time::now();
        pub_planner_waypoints_once = false;
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
        if(ros::Time::now()-last_path_send>ros::Duration(2.0))
        {
            pub_planner_waypoints_once = true;
        }
        
    }
    else
    {
        if(specified_yaw)
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,yaw,curr_plane_angle.z,true);
            vel_command_send(velocity_expected, attitude_expect.z);
            // vel_command_send(velocity_expected, 0);


        }
        else
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,0,curr_plane_angle.z,true);
            // vel_command_send(velocity_expected, attitude_expect.z);
            vel_command_send(velocity_expected, 0);
        }
    }
    planned_update_flag = false;

    double distance_to_go_nextpoint = sqrt((goal_msg.pose.position.x-current_point.x)*(goal_msg.pose.position.x-current_point.x)
                                                +(goal_msg.pose.position.y-current_point.y)*(goal_msg.pose.position.y-current_point.y)
                                                +(goal_msg.pose.position.z-current_point.z)*(goal_msg.pose.position.z-current_point.z));
    double yuzhi_temp;
    if(mission_number<=12)
    {
        yuzhi_temp = 0.5;
    }
    else
    {
        yuzhi_temp = 2.0;
    }
    if(distance_to_go_nextpoint<yuzhi_temp)
    {
        if(waypoint_exec_num<waypoints_plan.waypoints.size()-1)
        {
            waypoint_exec_num++;
            pub_planner_waypoints_once = true;
        }
        
    }
    else if(vehicle_static_time-vehicle_go_time>20.0)
    {
        vehicle_go_time = ros::Time::now().toSec();
        if(waypoint_exec_num<waypoints_plan.waypoints.size()-1)
        {
            waypoint_exec_num++;
            pub_planner_waypoints_once = true;
        }
    }

}

void simple_goal_planner(double target_x, double target_y, double target_z, double yaw, bool specified_yaw)
{
    if(pub_planner_goal_once)
    {
        geometry_msgs::PoseStamped goal_msg;
        goal_msg.pose.position.x = target_x;
        goal_msg.pose.position.y = target_y;
        goal_msg.pose.position.z = target_z;
        simple_goal_pub_.publish(goal_msg);
        last_pub_goal_time = ros::Time::now();
        pub_planner_goal_once = false;
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
        if(ros::Time::now()-last_pub_goal_time>ros::Duration(2.0))
        {
            planner_cnt++;
            pub_planner_goal_once = true;
            // std::cout<<"------>planner_cnt: "<<planner_cnt<<std::endl;
        }
        
    }
    else
    {
        if(specified_yaw)
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,yaw,curr_plane_angle.z,true);
            vel_command_send(velocity_expected, attitude_expect.z);
            // vel_command_send(velocity_expected, 0);
        }
        else
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,0,curr_plane_angle.z,true);
            // vel_command_send(velocity_expected, attitude_expect.z);
            vel_command_send(velocity_expected, 0);
        }
    }
    planned_update_flag = false;
}

void continuously_goal_planner(double target_x, double target_y, double target_z)
{
    double dist_to_goal = sqrt((target_x-current_point.x)*(target_x-current_point.x)+(target_y-current_point.y)*(target_y-current_point.y));
    if(ros::Time::now()-last_pub_tracking_time>ros::Duration(0.5)&&dist_to_goal>0.5)
    {
        geometry_msgs::PoseStamped goal_msg;
        goal_msg.pose.position.x = target_x;
        goal_msg.pose.position.y = target_y;
        goal_msg.pose.position.z = target_z;
        simple_goal_pub_.publish(goal_msg);
        last_pub_tracking_time = ros::Time::now();
        
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
    }
    else
    {
        position_pid_control(planned_point,current_point,max_vel_beishu+0.5,planned_yaw_angle,curr_plane_angle.z,true);
        // vel_command_send(velocity_expected, attitude_expect.z);
        vel_command_send(velocity_expected, 0);
    }
    planned_update_flag = false;
}

